#ifndef C_H_H
#define C_H_H
// #include <cstring>
#include <thread>
#include <chrono>
#include <vector>
#include <array>
#include <fstream>
#include "pid_controller.h"
#include <iostream>
#include <net/if.h>
#include <mutex>  // 添加此行
#include <atomic> // 添加此行以使用 std::atomic
#include "meun.h"
#include <opencv2/opencv.hpp>    // 添加 OpenCV 头文件
#include <sys/socket.h>          // 添加 socket 相关头文件
#include <arpa/inet.h>           // 添加 inet 相关头文件
#include <unistd.h>              // 添加 close 函数
#include <opencv2/imgcodecs.hpp> // 添加 OpenCV 图像编码头文件
#include <filesystem>
#include "motor.h"
#include "mohu.h"

#include "adrc.h"  // 添加ADRC头文件
#include "my_imagetransfer.h"
#include "zf_common_headfile.h" // 添加新的头文件

#include "flash.h"
#include <stdio.h>
#include <stdlib.h>
#include "isr.h"
#include "kalman.h"
#include <algorithm>  // 添加在文件头部

// 开关定义
#include <mutex>
#include <atomic>

#pragma once


/****************************************************************************************************/

#define GYRO
//#define yidui
#define erdui

/****************************************************************************************************/

// 图像大小定义
#define LCDW 186 // 图像宽度
#define LCDH 70  // 图像高度

#define MT9V03X_H 70
#define MT9V03X_W 186

#ifdef yidui
    #define SERVO_PWM_MAX 600.0f
#endif

#ifdef erdui
    #define SERVO_PWM_MAX 600.0f
#endif

#define byte uint8_t

// 舵机相关定义
#define SERVO_PWM "/dev/zf_device_pwm_servo"
#define BEEP "/dev/zf_driver_gpio_beep"
// 编码器定义
#define ENCODER_1 "/dev/zf_encoder_1"
#define ENCODER_2 "/dev/zf_encoder_2"
// 负压定义
#define PWM_1           "/dev/zf_device_pwm_esc_1"
#define PWM_2           "/dev/zf_device_pwm_esc_2"

//图像切换
// #define WCL
#define HWL

// PWM最大值定义
#define MOTOR1_PWM_DUTY_MAX (motor_1_pwm_info.duty_max)
#define MOTOR2_PWM_DUTY_MAX (motor_2_pwm_info.duty_max)
#define SERVO_PWM_DUTY_MAX (servo_pwm_info.duty_max)

#ifdef erdui
//二队
#define KEY_LEFT  "/dev/zf_driver_gpio_key_2"
#define KEY_UP    "/dev/zf_driver_gpio_key_0"
#define KEY_RIGHT "/dev/zf_driver_gpio_key_3"
#define KEY_ENTER "/dev/zf_driver_gpio_switch_0"
#define KEY_DOWN  "/dev/zf_driver_gpio_key_1"
#define KEY_BACK "/dev/zf_driver_gpio_switch_1"

#endif


#define long_z 1
#define duan_z 3
#define wan 2
#define other 4
#define ramp 5
#define huandao1 6
#define huandao2 7
#define huandao3 8
#define banmaxian 9
#define qibu 10
#define smalls 11
#define barrierr 12

// 0 cahngzhi
// 1 wandao
// 2 duanzhi

extern struct pwm_info motor_1_pwm_info;
extern struct pwm_info motor_2_pwm_info;
extern struct pwm_info servo_pwm_info;

// 声明全局变量
extern float Distance;
extern int cheku_1;
extern std::atomic<bool> should_exit;
extern int sock; // 添加socket全局变量定义
extern uint8_t tcp_suc_flag;
extern float pid_output;
extern uint8_t speed_ring_flag;
extern uint16_t ring_up_speed;
extern uint16_t ring_down_speed; // 环道测试速度
extern uint8_t banma_fache_flag; 
extern uint64_t during_time;
extern float distance;
extern float derivative;
extern uint8_t mohu_flag;
extern float chasu_k; // 差速系数
extern int ding_shi;
// 常量定义
extern float HIGH_SPEED_THRESHOLD; 
extern byte banma_up_hang;
extern byte banma_jieshu_cnt;
extern uint8_t tiaoshi_flag;
extern float fps;
extern uint8_t dj_flag;
//****************************图像处理****************************
//extern double otsuThreshold;
extern uint8_t BinPixels[LCDH][LCDW];
extern uint otsuMinThreshold; // 默认下限
extern uint otsuMaxThreshold; // 默认上限
extern uint otsuThreshold;
extern byte baseline;   // 基准线列
extern uint8_t end_hang;   // 基准线结束行
extern uint8_t baseline_l; // 上一次基准线列
extern uint8_t hang;
extern int lie;
extern uint8_t L_black[75], R_black[75], LCenter[75]; // 左边界和右边界
extern uint8_t rightflag[70];                         // 右线丢线标志
extern uint8_t leftflag[70];                          // 左线丢线标志

extern uint8_t Road_flag;
extern uint8_t wan_cnt;
extern uint8_t dajiao;
extern uint8_t dj;
extern uint8_t up_dj;
extern uint8_t down_dj;

extern int servo_duty;
extern uint16_t servo_base;
extern int image_error;

extern uint8_t left_huandao_flag, right_huandao_flag;
extern uint8_t huan_leftdown_x, huan_leftdown_y, huan_leftup_x, huan_leftup_y, huan_leftmid_x, huan_leftmid_y;
extern uint8_t huan_rightdown_x, huan_rightdown_y, huan_rightup_x, huan_rightup_y, huan_rightmid_x, huan_rightmid_y;
extern uint8_t leftp, rightp;
extern uint8_t kss, ksss;

extern uint8_t corner_flag;
//****************************图像处理****************************

// //电机控制
// void set_motor_control(int motor_dir_pin, int motor_pwm_pin, int pid_output);
// void control_motors(int pid_output_l, int pid_output_r);
// //速度环
// extern int16 encoder_left;
// extern int16 encoder_right;
// baohu
extern uint8_t road_state;
extern uint8_t switch_baohu;

// 开关定义
extern float time_lock; // 时间锁
extern float setPoint;    // 设定点
extern uint8_t zhushi_flag;
extern uint8_t sd_flag;
extern uint8_t chasu_flag;

extern int set_speed_low[6];
extern int set_speed_mid[6];
extern int set_speed_high[6];

// // 全局标志变量（原子操作保证线程安全）
// extern std::atomic<uint8_t> motor_flag;    // 电机控制标志
// extern std::atomic<uint8_t> run_flag;     // 运行标志
// extern std::atomic<uint8_t> serve_flag;   // 舵机标志
// // 互斥锁（保护非原子操作）
// extern std::mutex motor_mutex;
// // 时间戳
// extern std::chrono::time_point<std::chrono::high_resolution_clock> all_start_time;
extern uint8_t serve_flag;
//extern uint8_t motor_flag;
extern uint8_t run_flag;
extern uint8_t break_flag;

extern int pwm;

extern float raw_output_l;
extern float raw_output_r;
extern uint16_t chasu_xianfu;

// //速度环
// extern uint8_t speed_flag;
// extern float Speed_Diffen_xishu;  // 差速系数
// extern float Speed_Goal;               // 目标速度
// extern float Speed_Goal_l;                  // 左轮目标速度
// extern float Speed_Goal_r;                  // 右轮目标速度
// 目标速度
extern int huandao_speed, lz_speed, dz_speed, wan_speed,ramp_speed_up,ramp_speed_down;

// 全局变量声明
extern cv::VideoCapture cap;
// extern uint8_t BinPixle[LCDH][LCDW];
extern int sock;         // 添加socket全局变量声明
extern float Speed_Goal; // 目标速度


extern uint8_t banmaxian_flag;
extern uint8_t xie_shizi_flag_right; // 斜十字右
extern uint8_t xie_shizi_flag_left;  // 斜十字左
extern uint8_t zr_shizi_flag; // 环道右
extern byte hang_max;
// void isr_callback(void);
// void servo_control_callback(void);
// 函数声明
bool initCamera();
void getCompressedImage();
void drawBoundaryPoints();
void ImageProcess();
// 新增系统初始化和控制相关函数声明
void init_system(void);   // 系统初始化
void init_threads(void);  // 线程初始化
void init_network(void);  // 网络初始化
void run_main_loop(void); // 主循环函数
void run_loop(void);

// extern uint8_t L_black[MT9V03X_H];
// extern uint8_t R_black[MT9V03X_H];
// extern uint8_t LCenter[MT9V03X_H];
extern uint8_t right_break_flag[MT9V03X_H];
extern uint8_t left_break_flag[MT9V03X_H];
extern uint8_t L_expect[MT9V03X_H];
extern uint8_t R_expect[MT9V03X_H];

extern byte Up_limit;

extern byte left_D;
extern byte right_D;

// extern int corner_flag;
extern byte cross_flag;
extern byte ob_cross_flag;
extern int lefty_0;
extern int lefty_1;
extern int righty_0;
extern int righty_1;
extern int ob_righty_1;
extern int ob_lefty_1;
extern int ob_righty_0;
extern int ob_lefty_0;

extern int righty_up_island;
extern int lefty_up_island;
extern int righty_mid_island;
extern int lefty_mid_island;
extern int righty_down_island;
extern int lefty_down_island;

extern uint8 R_island_row;
extern uint8 L_island_row;
extern int righty_last_island;
extern int lefty_last_island;
extern int R_last_flag;
extern int L_last_flag;

extern byte Little_s_flag;

extern int ramp_flag;
extern int ramp_lefty;
extern int ramp_righty;
extern int ramp_count;

extern int cntl;
extern int cntr;

extern byte huandao_flag_right;
extern byte huandao_flag_left;
extern int hang_len_max_tf,xielv_use_tf;
// extern uint8_t BinPixels[70][186];

int out_fill_protect(void); // ���籣��
void Cross_flag_zero(void); // ʮ�ֱ�־λ����
float straight_k(int y1, int x1, int y2, int x2);

void Find_Baseline(void);               // Ѱ�һ�׼��
void Baseline_Sweepline(int startline); // ����ɨ��
int Returning_Line(int baseline);       // ���ߣ���ֹ���������ҹյ�
void Returning_Line_back(void);         // �����ٹ���
int lose_black(int type);               // Ѱ�Һ���
void set_speed_choice(uint8_t index);
int zebra_crossing_line_judge(int left_down, int right_down); // �������ж�
void zebra_crossing_line_fill();                              // �����߲���

int cross_patch_1(int type, int line);
int cross_patch_2(int type, int line);
int cross_patch_3(int type);
int cross_patch_5(int type, int line);
int cross_patch_6(int type);

int Corners_check(void);           // ����б�
int crossroad_check(int type);     // �б�ʮ��
int lose_road_maxnumber(int type); // ��������
int onepoint_loseline(void);       // ʮ��һ���յ㶪����

void crossroad_fill(void); // ʮ�ֲ���

int crossroad_find_left_up_point(void);    // Ѱ�����Ϲյ�
int crossroad_find_left_down_point(void);  // Ѱ�����¹յ�
int crossroad_find_right_up_point(void);   // Ѱ�����Ϲյ�
int crossroad_find_right_down_point(void); // Ѱ�����¹յ�
void deep_crossroad_fill(void);            // �������ʮ��״̬
void cross_state(void);                    // ʮ��״̬��

int Ob_onepoint_loseline(int type); // бʮ��һ���յ㶪����
void oblique_cross_fill(void);      // бʮ�ֲ���

int oblique_crossroad_find_left_up_point(void);    // Ѱ��б���Ϲյ�
int oblique_crossroad_find_right_up_point(void);   // Ѱ��б���Ϲյ�
int oblique_crossroad_find_left_down_point(void);  // Ѱ��б���¹յ�
int oblique_crossroad_find_right_down_point(void); // Ѱ��б���¹յ�

int Island_patch_1(int type, int line);
int Island_patch_2(int type); // ��һ���ǲ���ֱ��
int Island_patch_3(int type);
int Island_patch_4(int type);
void Island_patch_5(int type);
int Island_patch_6(int type);
int Island_patch_7(int type);
int Island_patch_8(int type);
int Island_patch_9(int type);
int Island_patch_10(int type, int line);
int Island_patch_11(int type);
int Island_patch_12(int type); // ���������Ҵ�
int Island_patch_13(int type, float k, float b);
int Island_patch_14(int type);
int Island_patch_15(int type);
int Island_patch_16(int type, int point);
int Island_patch_17(int type);
int Island_patch_18(int type);
int Island_patch_19(int type, int point);                  // ״̬1���Ҵ��յ�
int Island_patch_20(int type);                             // ״̬1���Ҵ��յ�
int Island_patch_21(int type, int startline, int endline); // ״̬3����״̬1_3����

void left_Island_fill(void);  // �󻷵�����
void right_Island_fill(void); // �һ�������

int find_island_left_up_point(void);    // Ѱ�һ������Ϲյ�
int find_island_right_up_point(void);   ////Ѱ�һ������Ϲյ�
int find_island_left_mid_point(void);   // Ѱ�һ������йյ�
int find_island_right_mid_point(void);  // Ѱ�һ������йյ�
int find_island_left_down_point(void);  // Ѱ�һ������¹յ�
int find_island_right_down_point(void); // Ѱ�һ������¹յ�

int little_s_patch_1(void); // ��Сs���״̬1������ֱ�ߣ�
int little_s_judge(void);   // СS���ж�
void little_s_state(void);  // Сs���ж�

void ramp_state(void); // �µ��ж�
int ramp_state_2_judge_1(void);
int ramp_state_2_judge_2(void);   // �µ�״̬2�ж�
int find_ramp_lefty_point(void);  // Ѱ���µ���յ�
int find_ramp_righty_point(void); // Ѱ���µ��ҹյ�

int long_straight_road(void);        // ��ֱ��
int straight_corner_judge(void);     // ֱ�价����
void straight_corner_separate(void); // ��Ԫ�ط���

void fill_patch_1(void);                                      // ���屣���Բ���
void Pixle_Filter(void);                                      // �������
void revise_slope(int line_limit);                            // ����б��
void straight_fill(int type, int y1, int x1, int y2, int x2); // б�߲���
void two_multiply_fill(int type, int startline, int endline); // б�߲���
void regression(int type, int startline, int endline);        // ��С���˷�������ߣ��ֱ�������ߣ����ߣ�����,type��ʾ����ļ�����   xy �ߵ�

int my_line_offset(int type, int startline, int endline); // �·��� 0-����  1-����   2-����
void Bezier_Line(byte type, int n);                        // �������������(LineΪԭ���ߣ�nΪ���鳤��)0

void refresh_middle(void); // ���²���

void draw_line(int type, int startline, int endline, float parameterB, float parameterA);
void Image_Deal();

void Calculate_Slope(int type, int starthang, int endhang);
int curve_judge(void);
int Check_straight1(int type, int startline, int endline);

#endif // C_H_H